#ifndef __ROBOT_KUKA_CKINEMATICS_H__
#define __ROBOT_KUKA_CKINEMATICS_H__

#include <robot/robot.h>
#include <robot/crobotpose.h>
#include <buola/algorithm/container.h>

namespace buola { namespace robot { namespace kuka {

inline void push_angles_between(std::vector<double> &pResult,double pAngle,double pMin,double pMax,double pInc=2*M_PI)
{
    if(pAngle<pMin)
    {
        do
        {
            pAngle+=pInc;
        } while(pAngle<pMin);
    }        
    else
    {
        while(pAngle>pMin+pInc)
            pAngle-=pInc;
    }

    while(pAngle<pMax)
    {
        pResult.push_back(pAngle);
        pAngle+=pInc;
    }
}

class CKinematics
{
public:
    static int GetNumJoints()	{	return 6;	}
    
    static double GetAlpha(int i)
    {
        static const double lAlpha[]={M_PI,-M_PI/2,0,M_PI/2,-M_PI/2,M_PI/2,M_PI};
        return lAlpha[i];
    }

    static double GetA(int i)
    {
        static const double lA[]={0,-0.075,-0.365,0.090,0,0,0};
        return lA[i];
    }

    static double GetD(int i)
    {
        static const double lD[]={0.203,-0.132,0,0,-0.405,0,-0.080};
        return lD[i];
    }

    static double GetTheta(int i)
    {
        static const double lTheta[]={0,M_PI,0,M_PI/2,0,0,M_PI};
        return lTheta[i];
    }

    static double GetJointMin(int i)
    {
        static const double lMin[]={deg2rad(-170.0),
                                    deg2rad(-190.0),
                                    deg2rad(-119.0),
                                    deg2rad(-190.0),
                                    deg2rad(-120.0),
                                    deg2rad(-350.0)};
        return lMin[i];
    }
    
    static double GetJointMax(int i)
    {
        static const double lMax[]={deg2rad(170.0),
                                    deg2rad(45.0),
                                    deg2rad(169.0),
                                    deg2rad(190.0),
                                    deg2rad(120.0),
                                    deg2rad(350.0)};
        return lMax[i];
    }

    static void SolveIK(std::vector<CRobotPose<CKinematics> > &pV,const mat::CVec3d &pPos,const mat::CQuat_d &pRot)
    {
        CRobotPose<CKinematics> lTestPose;
        
        mat::CVec3d lTargetZ=pRot*mat::CVec3d(0,0,1);
        mat::CVec3d lTargetJ5=pPos-lTargetZ*std::abs(GetD(6));
        
        //distance between joint 3 and 5 (fixed)
        double lDistJ3ToJ5Sq=GetD(4)*GetD(4)+GetA(3)*GetA(3);

        std::vector<double> lJ1s;
        push_angles_between(lJ1s,atan2(lTargetJ5.y(),-lTargetJ5.x()),GetJointMin(0),
                            GetJointMax(0),M_PI);

        for(int i1=0;i1<lJ1s.size();i1++)
        {
            //for each possible joint 1
            lTestPose[0]=lJ1s[i1];

            //get the position and rotation of the 2nd joint (fixed with respect to the first joint)
            mat::CVec3d lTempPos;
            mat::CQuat_d lTempRot;
            forward_kinematics(lTestPose,lTempPos,lTempRot,1);
            
            //get a normalized vector of the same direction as the kuka-arm and project it on the plane z=0
            mat::CVec3d lJoint2XYPlaneProjection=lTempRot*mat::CVec3d(-1,0,0);
            lJoint2XYPlaneProjection.z()=0;
            lJoint2XYPlaneProjection/=norm(lJoint2XYPlaneProjection);
            
            mat::CVec3d lJoint2To5=lTargetJ5-lTempPos;
            //angle from the horizontal to the vector between joint 2 and joint 5
            double lAlpha1=atan2(lTargetJ5.z()-lTempPos.z(),dot((lTargetJ5-lTempPos),lJoint2XYPlaneProjection));
            
            //if the distance from joint 2 to joint 5 is larger than the sum of link2 and the distance from joint 3 to joint 5, no solution exists. Check that.
            //angle between link between joints 2 and 3 and the vector from joint 3 to joint 5
            double lCosAlpha2=(GetA(2)*GetA(2)+norm2(lJoint2To5)-lDistJ3ToJ5Sq)/
                                (2*std::abs(GetA(2))*norm(lJoint2To5));
            if(std::abs(lCosAlpha2)>=1.0) continue;
            double lAlpha2=acos(lCosAlpha2);
            std::vector<double> lJ2s;
            push_angles_between(lJ2s,-lAlpha1-lAlpha2,GetJointMin(1),GetJointMax(1),2*M_PI);
            push_angles_between(lJ2s,-lAlpha1+lAlpha2,GetJointMin(1),GetJointMax(1),2*M_PI);

            for(int i2=0;i2<lJ2s.size();i2++)
            {
                lTestPose[1]=lJ2s[i2];
                
                double lCosBeta=(GetA(2)*GetA(2)+lDistJ3ToJ5Sq-norm2(lJoint2To5))/
                        (2*std::abs(GetA(2))*std::sqrt(lDistJ3ToJ5Sq));

                if(std::abs(lCosBeta)>1) continue;
                
                double lAngleJ4J5J3=atan2(std::abs(GetA(3)),std::abs(GetD(4)));
                
                double lBeta=acos(lCosBeta);

                std::vector<double> lJ3s;
                if(sin(lAlpha1+lJ2s[i2])>0)
                    push_angles_between(lJ3s,lBeta+lAngleJ4J5J3-M_PI,GetJointMin(2),GetJointMax(2),2*M_PI);
                else
                    push_angles_between(lJ3s,-lBeta+lAngleJ4J5J3+M_PI,GetJointMin(2),GetJointMax(2),2*M_PI);

                for(int i3=0;i3<lJ3s.size();i3++)
                {
                    lTestPose[2]=lJ3s[i3];
                    forward_kinematics(lTestPose,lTempPos,lTempRot,3);
                    mat::CVec3d lJ4ToJ5=lTempRot*mat::CVec3d(0,0,-1);
                    //vector in plane perpendicular to lJ4ToJ5 which determines orientation for joint 4
                    mat::CVec3d lCross=cross(lJ4ToJ5,lTargetZ);
                    double lDelta=atan2(dot(lCross,lTempRot*mat::CVec3d(1,0,0)),dot(lCross,lTempRot*mat::CVec3d(0,-1,0)));
                    
                    std::vector<double> lJ4s;
                    push_angles_between(lJ4s,lDelta,GetJointMin(3),GetJointMax(3),M_PI);

                    for(int i4=0;i4<lJ4s.size();i4++)
                    {
                        lTestPose[3]=lJ4s[i4];

                        forward_kinematics(lTestPose,lTempPos,lTempRot,4);
                        
                        double lAngle5=atan2(dot(lTargetZ,lTempRot*mat::CVec3d(-1,0,0)),dot(lTargetZ,lTempRot*mat::CVec3d(0,1,0)));
                    
                        std::vector<double> lJ5s;
                        push_angles_between(lJ5s,lAngle5,GetJointMin(4),GetJointMax(4),2*M_PI);

                        for(int i5=0;i5<lJ5s.size();i5++)
                        {
                            lTestPose[4]=lJ5s[i5];
                            
                            forward_kinematics(lTestPose,lTempPos,lTempRot,5);

                            mat::CVec3d lFinalX=pRot*mat::CVec3d(1,0,0);
                            
                            double lAngle6=atan2(dot(lFinalX,lTempRot*mat::CVec3d(0,-1,0)),dot(lFinalX,lTempRot*mat::CVec3d(-1,0,0)));

                            std::vector<double> lJ6s;
                            push_angles_between(lJ6s,lAngle6,GetJointMin(5),GetJointMax(5),2*M_PI);

                            for(int i6=0;i6<lJ6s.size();i6++)
                            {
                                lTestPose[5]=lJ6s[i6];

                                forward_kinematics(lTestPose,lTempPos,lTempRot,6);

                                double lTotalErr=norm(lTempPos-pPos)+std::abs(arg(conj(lTempRot)*pRot));
                                
                                if(lTotalErr>0.00001)
                                {
                                    msg_warn() << "large error in inverse kinematics " << lTotalErr << "\n"
                                               << "should be " << pPos << rad2deg(mat::C3DRotation(pRot)) << "\n"
                                               << "is " << lTempPos << rad2deg(mat::C3DRotation(lTempRot)) << "\n";
                                }
                                
                                pV.push_back(lTestPose); 
                            }
                        }
                    }
                }
            }
        }
    }

    static bool ComparePoses(const CRobotPose<CKinematics> &p1,const CRobotPose<CKinematics> &p2,const CRobotPose<CKinematics> &pRef)
    {
        //prefer positions that have joint 2 > -90.0
        if(pRef[1]>-M_PI_2)
        {
            if(p1[1]>-M_PI_2&&p2[1]<-M_PI_2)
                return true;
            if(p2[1]>-M_PI_2&&p1[1]<-M_PI_2)
                return false;
        }

        double l3J_1=0,l3J_2=0;
        
        for(int i=0;i<3;i++)
        {
            l3J_1+=std::abs(p1[i]-pRef[i]);
            l3J_2+=std::abs(p2[i]-pRef[i]);
        }
        
        if(l3J_1<l3J_2)
            return true;
        else if(l3J_2<l3J_1)
            return false;

        l3J_1=0;
        l3J_2=0;
        
        for(int i=3;i<6;i++)
        {
            l3J_1+=std::abs(p1[i]-pRef[i]);
            l3J_2+=std::abs(p2[i]-pRef[i]);
        }
        
        return (l3J_1<l3J_2);
    }
};

typedef CRobotPose<CKinematics> CArmPose;

/*namespace kuka*/ } /*namespace robot*/ } /*namespace buola*/ }

#endif
